#ifndef __STATE_H__
#define __STATE_H__

#include "util.h"
#include <vector>
#include <sstream>
#include <set>
#include <cstddef> // size_t

/** Directions that one can go toward.
 * O ----------------> Y
 * |         LEFT
 * |          ^
 * |          |
 * |  DOWN <--+--> UP
 * |          |
 * |          v
 * |        RIGHT
 * v
 * X
 * */
enum Direction
{
  RIGHT = 'v', // x=x+1, y=y
  UP    = '>', // x=x,   y=y+1
  LEFT  = '^', // x=x-1, y=y
  DOWN  = '<', // x=x,   y=y-1
  NDIRS = 4,   // number of available directions
  UNKNOWN_DIR = '?'
};

/// a single state of the search procedure
class State
{
private:
  Position pos_; ///< coordinate and type of this state
  Direction dir_; ///< direction to next state
public:
  State() : pos_(), dir_(UNKNOWN_DIR) {}
  /// copy constructor
  State(const State &st) : pos_(st.pos_), dir_(st.dir_) {}
  /// construct based on a map coordinate and its type
  State(const size_t x, const size_t y)
    : pos_(x, y), dir_(UNKNOWN_DIR) {}
  explicit State(const Position &pos) : pos_(pos), dir_(UNKNOWN_DIR) {}
  ~State() {}
public:
  State &operator=(const State &st)
    {
      if (this != &st) // avoid self-assignment
	{
	  pos_ = st.pos_;
	  dir_ = st.dir_;
	}
      return *this;
    }
  /// defined for STL set. 1st: x; 2nd: y; 3rd: dir
  bool operator<(const State &st) const
    {
      if (pos_.x() < st.pos_.x()) return true;
      if (st.pos_.x() < pos_.x()) return false;
      if (pos_.y() < st.pos_.y()) return true;
      if (st.pos_.y() < pos_.y()) return false;
      if (dir_ < st.dir_) return true;
      return false;
    }
  /// set dir_
  void dir(const Direction &d) { dir_ = d; }
  /// get dir_
  const Direction &dir() const { return dir_; }
  /// get coordinate X
  size_t x() const { return pos_.x(); }
  /// get coordinate Y
  size_t y() const { return pos_.y(); }
};

/// A directed diagram composed of a finite number of states
/// http://en.wikipedia.org/wiki/State_diagram
class StateDiagram
{
  /// Turn that one can take.
  enum Turn
    {
      CLOCKWISE, // turn right
      COUNTERCLOCKWISE // turn left
    };
private:
  TestMap map_; ///< original map
  std::vector<State> path_; ///< path from the start point to the goal
  Direction preferredDir_; ///< Preferred direction when moving to next state
  Turn preferredTurn_;   ///< how to take a turn when hitting a road block
  /// +1 when taking the preferred turn; -1 when taking a reverse turn;
  /// when numTurns_==0, keep moving forward along PreferredDir_.
  int numTurns_;
  std::set<State> stateCache_; ///< used to check state loop
  int has_loop_; ///< how many state loops have had before.
public:
  explicit StateDiagram(const TestMap &tmap) :
    map_(tmap),
    path_(1, State(map_.start())), // init current state with the start position
    numTurns_(0), // take no turn at the very beginning
    stateCache_(), has_loop_(0)
  { evaluatePreferredDirTurn(map_.goal()); }
  /// Destructor
  ~StateDiagram() {}
  /// Find a START-to-GOAL path in map_ with a similar method to
  /// the Pledge Algorithm:
  /// http://en.wikipedia.org/wiki/Maze_solving_algorithm#Pledge_algorithm
  bool searchPath(); ///< return true if found a path; return false otherwise.
  /// Print map_
  void print_map(const std::string &banner = "") const
    { map_.print(banner); }
  /// Print path_ over map_
  void print(const std::string &banner = "") const
    {
      TestMap mapCopy(map_); // Copy the original map
      for (std::vector<State>::const_iterator pit = path_.begin();
	   pit != path_.end(); ++pit) // Load the path to the map copy
	mapCopy.at(pit->x(), pit->y()) = pit->dir();
      // Load the START and GOAL
      mapCopy.at(map_.start().x(), map_.start().y()) =
	map_.at( map_.start().x(), map_.start().y());
      mapCopy.at(map_.goal().x(), map_.goal().y()) =
	map_.at( map_.goal().x(), map_.goal().y());
      mapCopy.print(banner); // print the modified map copy on the screen
    }
private:
  /// Evaluate preferred direction and turn
  void evaluatePreferredDirTurn(const Position &goal);
  /// Find the next position according to the moving direction.
  /// Stay at the same position if meeting a block or the map boundary.
  Position findNextPosition(const Direction &dir) const;
  /// Move to the next position.
  /// Return false if the next position is a block;
  /// Record the next state and return true, otherwise.
  bool moveTo(const Direction &dir);
  /// Return reference to current state
  State &currentState()
    { // avoid undefined behavior when path_ is empty
      assert( !path_.empty() || !"Cannot found the start position!");
      return path_.back();
    }
  const State &currentState() const
    { // avoid undefined behavior when path_ is empty
      assert( !path_.empty() || !"Cannot found the start position!");
      return path_.back();
    }
private:
  /// Return the new direction after taking a turn
  static Direction takeTurn(const Direction &current_dir, const Turn &turn)
    {
      switch (current_dir)
	{
	case RIGHT: return (turn != CLOCKWISE) ? UP    : DOWN;
	case UP:    return (turn != CLOCKWISE) ? LEFT  : RIGHT;
	case LEFT:  return (turn != CLOCKWISE) ? DOWN  : UP;
	case DOWN:  return (turn != CLOCKWISE) ? RIGHT : LEFT;
	default: assert(!"Unknown moving direction found!" && current_dir);
	}
      return UNKNOWN_DIR;
    }
  /// Return the reverse turn
  static Turn reverse(const Turn &turn)
    { return (turn==CLOCKWISE)? COUNTERCLOCKWISE: CLOCKWISE; }
  /// convert a number to a string
  static std::string numToStr(const double num)
    {
      std::ostringstream convert;
      convert << num;
      return convert.str();
    }
};

#endif //__STATE_H__
